#include <sys/time.h>
#include <sched.h>
#include <time.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdio.h>

#include "rt_utils.h"

extern int clock_nanosleep(clockid_t __clock_id, int __flags,
		           __const struct timespec *__req,
			   struct timespec *__rem);


#define NSEC_PER_SEC 1000000000ULL
static inline void timespec_add_us(struct timespec *t, uint64_t d)
{
    d *= 1000;
    d += t->tv_nsec;
    while (d >= NSEC_PER_SEC) {
        d -= NSEC_PER_SEC;
	t->tv_sec += 1;
    }
    t->tv_nsec = d;
}

void set_next_interarrival(struct periodic_task *t, int p)
{
  t->period = p;
}
void wait_next_activation(struct periodic_task *t)
{
    clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &t->r, NULL);
    timespec_add_us(&t->r, t->period);
}

struct periodic_task *start_periodic_timer(uint64_t offs, int p)
{
    struct periodic_task *t;

    t = malloc(sizeof(struct periodic_task));
    if (t == NULL) {
        return NULL;
    }
    clock_gettime(CLOCK_REALTIME, &t->r);
    timespec_add_us(&t->r, offs);
    t->period = p;

    return t;
}
int make_rt(void)
{
  int res;
  struct sched_param param;

  param.sched_priority = 1;
  res = sched_setscheduler (0, SCHED_FIFO, &param);
  if (res != 0) {
    perror ("sched_setscheduler");
    printf ("You probably need to be running as root.\n");

    return -1;
  }

  return 0;
}
